#include "unionsys_core.hpp"

bool ifregistered = false;
bool nonx = false;
std::string white_list[] = {"1421121005334", //unionsys 01
                            "1422420034992", //quanquan 01
                            "1421121005202", //quanquan 02
                            "1421121005347", //quanquan 03
                            "14230190217090c00103",  //xgd-250-01
			    			"14203210604420400503",  //xgd-250-02
							"" };
                            
RegisterMSN::RegisterMSN() {
    ifregistered = false;
}

RegisterMSN::~RegisterMSN() {
    ifregistered = false;
}

int RegisterMSN::get_dds_pub_size()
{
    int out;
    if(ifregistered)
    {
        //30 the largest size for one pack 30
        out = 30;
    }
    return out;
}

//
geometry_msgs::Vector3 RegisterMSN::calc_P_offset(Eigen::Matrix3d R_cam_tag, geometry_msgs::Point P_cam_tag, geometry_msgs::Vector3 P_plane_cam, 
                                    geometry_msgs::Point P_plane_home, Eigen::Matrix3d R_tag_world, geometry_msgs::Vector3 P_tag_world)
{   
    geometry_msgs::Vector3 P_offset;

    if(ifregistered)
    {
        //P_plane_world =  R_t_w*P_c_t + P_t_w + R_t_w*R_c_t*P_p_c
        geometry_msgs::Vector3 RctXPpc = R_multi_P(R_cam_tag, P_plane_cam.x, P_plane_cam.y, P_plane_cam.z);
        geometry_msgs::Vector3 RtwXRctXPpc = R_multi_P(R_tag_world, RctXPpc.x, RctXPpc.y, RctXPpc.z);
        geometry_msgs::Vector3 RtwXPct = R_multi_P(R_tag_world, P_cam_tag.x, P_cam_tag.y, P_cam_tag.z);

        geometry_msgs::Vector3 P_plane_world; 
        P_plane_world.x = RtwXRctXPpc.x + RtwXPct.x + P_tag_world.x;
        P_plane_world.y = RtwXRctXPpc.y + RtwXPct.y + P_tag_world.y;
        P_plane_world.z = RtwXRctXPpc.z + RtwXPct.z + P_tag_world.z;

        P_offset.x = P_plane_world.x - P_plane_home.x;
        P_offset.y = P_plane_world.y - P_plane_home.y;
        P_offset.z = P_plane_world.z - P_plane_home.z;
    }
    
    return P_offset;
}

geometry_msgs::Quaternion RegisterMSN::calc_Q_offset(geometry_msgs::Quaternion Q_cam_tag, geometry_msgs::Quaternion Q_tag_world, geometry_msgs::Quaternion Q_plane_cam, geometry_msgs::Quaternion Q_plane_home)
{
    geometry_msgs::Quaternion Q_offset;
    if(ifregistered)
    {
        //Q_p_w = Q_p_c*Q_c_t*Q_t_w
        geometry_msgs::Quaternion QctXQtw = quatMultiply(Q_cam_tag, Q_tag_world);
        geometry_msgs::Quaternion Q_plane_world = quatMultiply(Q_plane_cam, QctXQtw);
        //Q_p_w = Q_offset*Q_p_h
        //Q_offset = Q_p_h_i*Q_p_w
        geometry_msgs::Quaternion Q_plane_home_inverse;
        Q_plane_home_inverse.x = -Q_plane_home.x;
        Q_plane_home_inverse.y = -Q_plane_home.y;
        Q_plane_home_inverse.z = -Q_plane_home.z;
        Q_plane_home_inverse.w = Q_plane_home.w;
        Q_offset = quatMultiply(Q_plane_home_inverse, Q_plane_world);
    }
    
    return Q_offset;
}



geometry_msgs::Quaternion RegisterMSN::quatMultiply(geometry_msgs::Quaternion q2, geometry_msgs::Quaternion q1) {
    geometry_msgs::Quaternion qr;

    qr.w = q1.w*q2.w-q1.x*q2.x-q1.y*q2.y-q1.z*q2.z;
    qr.x = q1.w*q2.x+q1.x*q2.w+q1.y*q2.z-q1.z*q2.y;
    qr.y = q1.w*q2.y-q1.x*q2.z+q1.y*q2.w+q1.z*q2.x;
    qr.z = q1.w*q2.z+q1.x*q2.y-q1.y*q2.x+q1.z*q2.w;

    return qr;
}

Eigen::Matrix3d RegisterMSN::Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)  
{  
    Eigen::Quaterniond q;  
    q.x() = x;  
    q.y() = y;  
    q.z() = z;  
    q.w() = w;  
  
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();   
    return R;  
} 

geometry_msgs::Vector3 RegisterMSN::R_multi_P(Eigen::Matrix3d R, double x, double y, double z)
{
    geometry_msgs::Vector3 output;
    output.x = R(0,0)*x+R(0,1)*y+R(0,2)*z;
    output.y = R(1,0)*x+R(1,1)*y+R(1,2)*z;
    output.z = R(2,0)*x+R(2,1)*y+R(2,2)*z;
    return output;

}

Eigen::Matrix3d RegisterMSN::R_multi_R(Eigen::Matrix3d R1, Eigen::Matrix3d R2)
{
    Eigen::Matrix3d output;

    output(0,0) = R1(0,0)*R2(0,0)+R1(0,1)*R2(1,0)+R1(0,2)*R2(2,0);
    output(0,1) = R1(0,0)*R2(0,1)+R1(0,1)*R2(1,1)+R1(0,2)*R2(2,1);
    output(0,2) = R1(0,0)*R2(0,2)+R1(0,1)*R2(1,2)+R1(0,2)*R2(2,2);
    output(1,0) = R1(1,0)*R2(0,0)+R1(1,1)*R2(1,0)+R1(1,2)*R2(2,0);
    output(1,1) = R1(1,0)*R2(0,1)+R1(1,1)*R2(1,1)+R1(1,2)*R2(2,1);
    output(1,2) = R1(1,0)*R2(0,2)+R1(1,1)*R2(1,2)+R1(1,2)*R2(2,2);
    output(2,0) = R1(2,0)*R2(0,0)+R1(2,1)*R2(1,0)+R1(2,2)*R2(2,0);
    output(2,1) = R1(2,0)*R2(0,1)+R1(2,1)*R2(1,1)+R1(2,2)*R2(2,1);
    output(2,2) = R1(2,0)*R2(0,2)+R1(2,1)*R2(1,2)+R1(2,2)*R2(2,2);

    return output;

}




//拆分 str 字符串
std::vector<std::string> RegisterMSN::deserial_str(std::string str)
{
    //get the drone id and flight state,formate: id/state
    std::vector<std::string> res_temp;
    if(ifregistered)
    {
        const char split = '/';
        std::istringstream iss(str);	// 输入流
        std::string token;			// 接收缓冲区
        while (std::getline(iss, token, split))	// 以split为分隔符
        {
            res_temp.push_back(token);
        }
    }
    return res_temp;
}

//合并 str 字符串
std::string RegisterMSN::serial_str(int drone_id, int mission_state)
{
    std::string str;
    if(ifregistered)
    {
        str =  std::to_string(drone_id) + std::string("/") + std::to_string(mission_state);

    }
    return str;
}

//计算camera pos
geometry_msgs::PoseStamped RegisterMSN::calculate_cam_pos(nav_msgs::Odometry current_info, Eigen::Matrix3d Rroll, Eigen::Matrix3d Rpitch)
{
    geometry_msgs::PoseStamped pose_out;
    if(ifregistered)
    {
        pose_out.pose.position.x = current_info.pose.pose.position.x;
        pose_out.pose.position.y = current_info.pose.pose.position.y;
        pose_out.pose.position.z = current_info.pose.pose.position.z;
        Eigen::Matrix3d R_curr = Quaternion2RotationMatrix(current_info.pose.pose.orientation.x, current_info.pose.pose.orientation.y,
                                                             current_info.pose.pose.orientation.z,current_info.pose.pose.orientation.w);
        Eigen::Matrix3d R_Xpitch = R_multi_R(R_curr, Rroll);
        Eigen::Matrix3d R_Xroll = R_multi_R(R_Xpitch, Rpitch);
        Eigen::Quaterniond quat = Eigen::Quaterniond(R_Xroll);
        pose_out.pose.orientation.x=quat.x();
        pose_out.pose.orientation.y=quat.y();
        pose_out.pose.orientation.z=quat.z();
        pose_out.pose.orientation.w=quat.w();
        
    }
    return pose_out;   
}

geometry_msgs::PoseStamped RegisterMSN::calculate_cam_pos(nav_msgs::Odometry current_info, float roll, float pitch , float yaw)
{
    geometry_msgs::PoseStamped pose_out;
    if(ifregistered)
    {
        pose_out.pose.position.x = current_info.pose.pose.position.x;
        pose_out.pose.position.y = current_info.pose.pose.position.y;
        pose_out.pose.position.z = current_info.pose.pose.position.z;
        pose_out.pose.orientation.x=current_info.pose.pose.orientation.x;
        pose_out.pose.orientation.y=current_info.pose.pose.orientation.y;
        pose_out.pose.orientation.z=current_info.pose.pose.orientation.z;
        pose_out.pose.orientation.w=current_info.pose.pose.orientation.w;
        geometry_msgs::Vector3 current_test_angle = toEulerAngle(pose_out.pose.orientation);
        geometry_msgs::Quaternion quaternion_test=tf::createQuaternionMsgFromRollPitchYaw(current_test_angle.x+roll,current_test_angle.y+pitch,current_test_angle.z+yaw);
        
        pose_out.pose.orientation.x=quaternion_test.x;
        pose_out.pose.orientation.y=quaternion_test.y;
        pose_out.pose.orientation.z=quaternion_test.z;
        pose_out.pose.orientation.w=quaternion_test.w;
        
    }
    return pose_out;   
}

geometry_msgs::Vector3 RegisterMSN::toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;
    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
    return ans;
}

//计算相机偏差  Ppg=Tcg*Ppc => Ppg = Rcg*Ppc + tcg ,  -Rcg*Rcp*Pcp 就是这个函数要实现的功能
geometry_msgs::Point RegisterMSN::rotate_Rcg_Ppc(geometry_msgs::Vector3 angle_cg, geometry_msgs::Vector3 angle_cp, float delta_x, float delta_y, float delta_z)
{
    geometry_msgs::Point delta_distance;
    if(ifregistered)
    {
        geometry_msgs::Quaternion quaternion_cg=tf::createQuaternionMsgFromRollPitchYaw(angle_cg.x,angle_cg.y,angle_cg.z);
        geometry_msgs::Quaternion quaternion_cp=tf::createQuaternionMsgFromRollPitchYaw(angle_cp.x,angle_cp.y,angle_cp.z);

        Eigen::Matrix3d rotation_cg = Quaternion2RotationMatrix(quaternion_cg.x, quaternion_cg.y, quaternion_cg.z, quaternion_cg.w);
        Eigen::Matrix3d rotation_cp = Quaternion2RotationMatrix(quaternion_cp.x, quaternion_cp.y, quaternion_cp.z, quaternion_cp.w);

        geometry_msgs::Vector3 P_pc = R_multi_P(rotation_cp, delta_x, delta_y, delta_z);
        geometry_msgs::Vector3 P_out = R_multi_P(rotation_cg, -P_pc.x, -P_pc.y, -P_pc.z);

        delta_distance.x = P_out.x;
        delta_distance.y = P_out.y;
        delta_distance.z = P_out.z;

    }
    
    return delta_distance;
}

//计算相机偏差  
geometry_msgs::Point RegisterMSN::rotation_hehe(geometry_msgs::Vector3 angle, float delta_x, float delta_y, float delta_z)
{
    geometry_msgs::Point delta_distance;
    if(ifregistered)
    {

        float Rbe[3][3] = {0};
        Rbe[0][0] = cos(angle.y) * cos(angle.z);
        Rbe[0][1] = cos(angle.z) * sin(angle.y) * sin(angle.x) - sin(angle.z) * cos(angle.x);
        Rbe[0][2] = cos(angle.z) * sin(angle.y) * cos(angle.x) + sin(angle.z) * sin(angle.x);
        Rbe[1][0] = cos(angle.y) * sin(angle.z);
        Rbe[1][1] = sin(angle.z) * sin(angle.y) * sin(angle.x) + cos(angle.z) * cos(angle.x);
        Rbe[1][2] = sin(angle.z) * sin(angle.y) * cos(angle.x) - cos(angle.z) * sin(angle.x);
        Rbe[2][0] = -sin(angle.y);
        Rbe[2][1] = sin(angle.x) * cos(angle.y);
        Rbe[2][2] = cos(angle.x) * cos(angle.y);
        delta_distance.x = Rbe[0][0] * delta_x + Rbe[0][1] * delta_y + Rbe[0][2] * delta_z;
        delta_distance.y = Rbe[1][0] * delta_x + Rbe[1][1] * delta_y + Rbe[1][2] * delta_z;
        delta_distance.z = Rbe[2][0] * delta_x + Rbe[2][1] * delta_y + Rbe[2][2] * delta_z;
    }
    
    return delta_distance;
}


//limit speed
geometry_msgs::Point RegisterMSN::limit_velocity(float vx, float vy,float maximum)
{
	geometry_msgs::Point vel;
    if(ifregistered)
    {
        float velocity = sqrt(vx * vx + vy * vy);
        if(maximum <= 0)
        {
            vel.x = 0;
            vel.y = 0;
        }
        if(velocity <= maximum)
        {
            vel.x = vx;
            vel.y = vy;
        }
        //if velocity is bigger than maximum, then limit the vx and vy, and keep the direction at meanwhile.
        else
        {
            //the velocity must not be zero when in this step
            vel.x = vx / velocity * maximum;
            vel.y = vy / velocity * maximum;
        }
    }
	return vel;
}

//fisheye model
double RegisterMSN::fisheye_model_dist2pitch(double dist)
{
    if(ifregistered)
    {
        double dist_out = 90.0-0.2621*dist;
        return dist_out;
    }
    return 0;
    
}

double RegisterMSN::fisheye_model_focus(double pitch_deg)
{
    if(ifregistered)
    {
        double virtual_focus = -0.01553*pitch_deg*pitch_deg+0.386*pitch_deg+448.8;
        return virtual_focus;
    }
    return 0;
    
}

// 设备序列号检测
void RegisterMSN::detect_serial_number()
{
    //自用
    ifregistered = true;


	// FILE *pp = popen("cat /sys/firmware/devicetree/base/serial-number", "r"); // build pipe
	// if (!pp)
    // {
    //     nonx = true;
    //     printf("====>erro open serial-number file!\n");
    //     return;
    // }
 
	// // collect cmd execute result
	// char tmp[1024];
	// while (fgets(tmp, sizeof(tmp), pp) != NULL)
    // {
	// 	// std::cout << tmp << std::endl; // can join each line as string
    //     std::cout << "\n";
    // }
	// pclose(pp);
    // std::string serial_number = tmp;

    // //compare with white list
    // bool cmp_temp = false;
    // for(std::string index:white_list)
    // {
    //     if(serial_number == index)
    //     {
    //         cmp_temp =  true;
    //     }
    // }

    // if(cmp_temp)  //nx
    // {
    //     printf("====>registered return true!\n");
    //     ifregistered = true;
    // }
    // else
    // {
    //     printf("===>registered return false!,all outputs will be zero!! contact me by 759562862@qq.com \n");
    //     ifregistered = false;
    // }
    

}


void RegisterMSN::print_var_reg()
{
    std::cout<<"=====>the var reg returns:"<< ifregistered <<std::endl;
}

//password
void RegisterMSN::print_var_reg(int key)
{
    if(nonx)
    {
        if(key==19951127)  //nx
        {
            printf("====>registered pc return true!\n");
            ifregistered = true;
        }
        else
        {
            printf("===>registered pc return false!,all outputs will be zero!! contact me by 759562862@qq.com \n");
            ifregistered = false;
        }
    }
    
}


 

